/*********************************************************************
*
*   stu_main.cpp - Student main module
*
*   Copyright:
*       Software source code by Alex Morozov and Chris D. Locke is
*       licensed under a Creative Commons Attribution-Noncommercial-
*       Share Alike 3.0 United States License
*       (http://creativecommons.org/licenses/by-nc-sa/3.0/us/)
*
*********************************************************************/

// Call SetLeftMotor(left_speed) and SetRightMotor(right_speed) to control robot movement
// left_speed and right_speed should be between -1.0 and 1.0

// Challenge A:
//  Modify StudentRobot::StudentPeriodic() to drive robot in a square using compass and encoder
//  To read compass, use m_compass->GetAngle()
//  To read encoder, use m_leftEncoder and m_rightEncoder
//  m_leftEncoder->Start() restarts pulse counting
//  m_leftEncoder->Get() returns the number of pulses since you called Start()

// Challenge B:
//  Modify StudentRobot::StudentPeriodic() to drive robot using joystick
//  To read joystick, use m_rightStick->GetX() and m_rightStick->GetY()

// Challenge C:
//  Use joystick to drive the robot around.
//  When joystick returns to origin (0,0), automatically drive robot to X=0,Y=0
//  You can get current position by calling GetRobotPosition( &x, &y, &heading )
//  x, y, heading should be float


#include <math.h>
#include "DigitalInput.h"
#include "DigitalOutput.h"
#include "Encoder.h"
#include "HiTechnicCompass.h"
#include "Joystick.h"
#include "RobotBase.h"

#define AUTOMODE 2

/*********************************************************************
*
*   CLASS NAME:
*       StudentRobot - Student Robot Code
*
*   DESCRIPTION:
*       All student code goes here.
*
*********************************************************************/

class StudentRobot : public RobotBase
{
public:
    StudentRobot( );

protected:
    virtual void StudentPeriodic( );

private:
    unsigned int        t_ms;       /* milliseconds since start     */

    HiTechnicCompass *  m_compass;  /* simulated compass            */
    DigitalOutput *     m_arm_output;
                                    /* simulated arm output         */
    DigitalOutput *     m_blink_led;/* blink once per second        */
    Joystick *          m_rightStick;
                                    /* right joystick input         */
    DigitalInput *      m_switch;   /* simulated switch input       */

    Encoder *           m_leftEncoder;
                                    /* left wheel encoder           */
    Encoder *           m_rightEncoder;
                                    /* right wheel encoder          */
	int					automode;
	int					start; 
};

/*********************************************************************
*
*   PROCEDURE NAME:
*       StudentRobot::StudentRobot - Student Robot Constructor
*
*   DESCRIPTION:
*       This function is called when the robot is created
*
*********************************************************************/

StudentRobot::StudentRobot( )
{

    /*------------------------------------------------------
    Start at time zero
    ------------------------------------------------------*/
    t_ms = 0;

    /*------------------------------------------------------
    Create members
    ------------------------------------------------------*/
    m_rightStick = new Joystick( 1 );
    m_blink_led  = new DigitalOutput( 1 );
    m_compass    = new HiTechnicCompass( 1 );
    m_leftEncoder = new Encoder( 1, 1 );
    m_rightEncoder = new Encoder( 2, 2 );

    // Start the encoders
    m_leftEncoder->Start();
    m_rightEncoder->Start();
    m_switch     = new DigitalInput( 6, 1 );
    m_arm_output = new DigitalOutput( 2 );
	
	#if AUTOMODE == 1
		//First Position
		SetPos( -293, -215 ); 
		SetAngle( 265 );

	#elif AUTOMODE == 2
		//Middle Position
		SetPos( 145, -88 ); 
		SetAngle( 270 ); 

	#elif AUTOMODE == 3
		//Far Position
		SetPos( 612, -88 ); 
		SetAngle( 270 );    
	#endif
}   /* StudentRobot::StudentRobot() */


/*********************************************************************
*
*   PROCEDURE NAME:
*       StudentRobot::StudentPeriodic - Periodic Function
*
*   DESCRIPTION:
*       This function is called at 200Hz (every 5ms) by the simulation
*       code.
*       Control your robot from here.
*       Use SetLeftMotor() and SetRightMotor() to set motor speed.
*
*********************************************************************/

void StudentRobot::StudentPeriodic( )
{
    static float        rt_spd = 0.0f;

    /*------------------------------------------------------
    Increment millisecond counter based on periodic rate
    ------------------------------------------------------*/
    t_ms += 5;

    /*------------------------------------------------------
    This is how we read compass
    ------------------------------------------------------*/
    float heading = m_compass->GetAngle();

    /*------------------------------------------------------
    Get joystick inputs -1.0 to 1.0
    ------------------------------------------------------*/
    double              x, y;
    x = ( double )m_rightStick->GetX( );
    y = ( double )m_rightStick->GetY( );

    /*------------------------------------------------------
    Example: set arm output based on switch input
    ------------------------------------------------------*/
    m_arm_output->Set( m_switch->Get( ) );

    /*------------------------------------------------------
    This is how we read encoders
    ------------------------------------------------------*/
    int leftPulseCnt = m_leftEncoder->Get();
    int rightPulseCnt = m_rightEncoder->Get();

    /*------------------------------------------------------
    This is how we control digital output
    ------------------------------------------------------*/
    //if( 0 == ( t_ms % 1000 ) )
    //    {
    //    m_blink_led->Set( ( t_ms / 1000 ) & 1 );
    //    }

    /*------------------------------------------------------
    To show an example of how to control the robot,
    oscillate motors 90 degrees out of phase with each other
    ------------------------------------------------------*/
    //SetLeftMotor( y + x );
    //SetRightMotor( y - x );
    //SetLeftMotor( y + x );
    //SetRightMotor( y - x );

	//if (t_ms == 5)
	//{
	//	SetLeftMotor( 1.0 );
	//	SetRightMotor( 1.0 );
	//}
	//if ( leftPulseCnt > 1600)
	//{
	//	SetLeftMotor( 0.0 );
	//	SetRightMotor( 0.0 );
	//}

#if AUTOMODE == 1
	
	if (t_ms == 5)
	{
		SetLeftMotor( 1.0 );
		SetRightMotor( 1.0 );
	}
	if ( leftPulseCnt > 1600)
	{
		SetLeftMotor( 0.0 );
		SetRightMotor( 0.0 );
	}
	#elif AUTOMODE == 2

		if (t_ms == 5)
		{
			SetLeftMotor( 0.5 );
			SetRightMotor( 0.5 );
		}

		else if ( leftPulseCnt >= 200 && rightPulseCnt < 349)
		{
			SetLeftMotor( 0.0 );
			SetRightMotor( 0.5 );
		}

		else if ( leftPulseCnt >= 200 && rightPulseCnt > 349  && rightPulseCnt <= 780)
		{
			SetLeftMotor( 0.5 );
			SetRightMotor( 0.5 );
		}

		else if ( (rightPulseCnt > 780) && (leftPulseCnt < 773) )
		{
			SetLeftMotor( 0.5 );
			SetRightMotor( 0.0 );
		}

			else if ( (rightPulseCnt > 780) && (leftPulseCnt > 773) && (rightPulseCnt < 1450 ))
		{
			SetLeftMotor( 0.5 );
			SetRightMotor( 0.5 );
		}


		else if ( (rightPulseCnt > 780) && (leftPulseCnt > 773) )
		{
			SetLeftMotor( 0.0 );
			SetRightMotor( 0.0 );
		}
								
	#elif AUTOMODE == 3
		//Far Position
		SetPos( 612, -88 ); 
		SetAngle( 270 );
	#endif
	

	
}   /* StudentRobot::StudentPeriodic */


StudentRobot robot;
